//
// Created by Aron on 2024/10/31.
//

#include "Yaw.h"
#include "Speed_control.h"
#include "CAN_receive.h"
pid_type_def pid_speed_Yaw;
pid_type_def pid_angle_Yaw;
static float hlimit_Y;
static float llimit_Y;
static float n_Y=4.3945f;
static int16_t max_speed_change_Y=2000;
int YawFlag=1;
static float set_angle;
static float ref_angle;
void Yaw_control(int16_t angle) {
    if (YawFlag){
        set_angle=motor_measure[4].total_ecd*n_Y;
        YawFlag=0;
    }
    else set_angle+=angle;
    ref_angle=motor_measure[4].total_ecd*n_Y;
//    if (set_angle>hlimit_Y) {
//        set_angle=hlimit_Y;
//    }else if(set_angle<llimit_Y) {
//        set_angle=llimit_Y;
//    }
    PID_calc(&pid_angle_Yaw, ref_angle, set_angle);
    Speed_out(pid_angle_Yaw.out, motor_measure[4].speed_rpm, max_speed_change_Y,
    &pid_speed_Yaw);
    CAN2_cmd(pid_speed_Yaw.out);
}